#include "main.h"
#include "Graphics/Graphics.h"

#include <fstream>
/*---------------------------------------------------------------------------*/
     /* Particle Data Arrays (PDA) store in Global memory on HOST */
     float3    d_Particle_Pos    [2097152]; /* <--DEVICE */
     Quaterion d_Particle_Quart  [2097152]; /* <--DEVICE */
     int       d_P_ID            [2097152]; /* <--DEVICE */
     int       d_Particle_Type   [2097152]; /* <--DEVICE */
/*---------------------------------------------------------------------------*/

ofstream PDA;
fstream  PDAFIN;


float3    d_Particle_PosD; /* <--DEVICE */
int       d_Particle_TypeD; /* <--DEVICE */
Quaterion axisangle ;

/*---------------------------------------------------------------------------*/
      /* Entry point of the CODE everything is called from here */
/*---------------------------------------------------------------------------*/
void display1()
{


 //PDA.open("DYNA.txt");
 while( t <= 2.0f)
 {
//	frame++;
//	if(frame==8)
//	{
//
//      /* 1) Call to the DEVICE to get system state */
//      m_KDevice->IDevice_Get_Positions( d_Particle_Pos, d_Particle_Type ,d_Particle_Quart,d_P_ID);
//
//   //   PDA<<""<<endl;
//      for ( int i=0; i<SimInfo.Num_Particles; i++ )
//      {
//
//         d_Particle_Pos[i]= d_Particle_Pos[i] - m_KParticleObject[ d_Particle_Type[i] ].COM;
//         axisangle = quart_axisAngleH(d_Particle_Quart[i]);
//
//         PDA<<d_Particle_Type[i]<<" "<<d_Particle_Pos[i].x <<" "<<d_Particle_Pos[i].y<<" "<<d_Particle_Pos[i].z<<"  "
//        		 <<axisangle.w<< " "<< axisangle.x<< " "<< axisangle.y<<" "<< axisangle.z<<endl;
//
//       }
//
//	   frame = 0;
//
//    } /* frame check */
//

    /*--------------------------------------------------------------------------------*/
                          /* Device Interaction */
    /*--------------------------------------------------------------------------------*/

   	  /* 2.1) DEVICE calculates the forces and net velocity on all particles */
   	    m_KDevice->IDevice_Body_Body_Interaction();

   	  /* 2.2) DEVICE checks for collision with World and INTEGRATES */
   	    m_KDevice->IDevice_World_Interaction();

   	    t +=  (SimInfo.InitalDelta_t)*0.1f; /* Update counter */
        printf("  %f \r",t);
  }

//PDA.close();
}

void display2(void)
{

/*--------------------------------------------------------------------------------*/
                           /* Graphics Call */
/*--------------------------------------------------------------------------------*/
  GL_DisplaySet();
     /* Storage of Particle Positions */


  //float3 Particle_Rot  [SimInfo.Num_Particles]; /* <--DEVICE */

 /* Draw World Geometry */
  glPushMatrix();
	glScalef( 0.25f, 0.25f, 0.25f );
	glCallList(1);
  glPopMatrix();
 /*--------------------------------------------------------------------------------*/


 /*--------------------------------------------------------------------------------*/
                          /* Display on Screen */
 /*--------------------------------------------------------------------------------*/
  if(!Gl_bPause )
  {

   for ( int i=0; i<SimInfo.Num_Particles; i++ )
   {


	    PDAFIN>>d_Particle_TypeD>>d_Particle_PosD.x>>d_Particle_PosD.y>>d_Particle_PosD.z>>
	    		axisangle.w>>axisangle.x>>axisangle.y>>axisangle.z;

         glPushMatrix();
         glScalef(0.25f,0.25f,0.25f);
         glTranslatef( d_Particle_PosD.x,d_Particle_PosD.y, d_Particle_PosD.z);
         //glCallList(20+d_P_ID[i]); /* color levels */
         glTranslatef( m_KParticleObject[ d_Particle_TypeD ].COM.x,m_KParticleObject[ d_Particle_TypeD].COM.y,m_KParticleObject[ d_Particle_TypeD].COM.z);
         glRotatef(axisangle.w,axisangle.x,axisangle.y,axisangle.z);
      //   glTranslatef( -m_KParticleObject[ d_Particle_TypeD ].COM.x,-m_KParticleObtype filter textject[ d_Particle_TypeD ].COM.y,-m_KParticleObject[ d_Particle_TypeD].COM.z);
         glCallList(10+d_Particle_TypeD);
         glPopMatrix();

    }


	  glFlush();
      glutSwapBuffers();

  }

    num_timeStep++;
     /* Print actual time to window */
    sprintf(fps, "%s Elapsed Time ): %3.1f s",num_timeStep*0.05f);
    glutSetWindowTitle(fps);

   glutPostRedisplay();

}

/*---------------------------------------------------------------------------*/
      /* Entry point of the CODE everything is called from here */
/*---------------------------------------------------------------------------*/
void display()
{

/*--------------------------------------------------------------------------------*/
                           /* Graphics Call */
/*--------------------------------------------------------------------------------*/
  GL_DisplaySet();
     /* Storage of Particle Positions */


  //float3 Particle_Rot  [SimInfo.Num_Particles]; /* <--DEVICE */

 /* Draw World Geometry */
  glPushMatrix();
	glScalef( 0.25f, 0.25f, 0.25f );
	glCallList(1);
  glPopMatrix();
 /*--------------------------------------------------------------------------------*/
 frame++;

 if(frame==20)
 {

    /* 1) Call to the DEVICE to get system state */
    m_KDevice->IDevice_Get_Positions( d_Particle_Pos, d_Particle_Type ,d_Particle_Quart,d_P_ID);

    Quaterion axisangle ;
   /*--------------------------------------------------------------------------------*/
                          /* Display on Screen */
   /*--------------------------------------------------------------------------------*/

   /* Loop through all particles and update there position on screen */

   for ( int i=0; i<SimInfo.Num_Particles; i++ )
   {

	     axisangle = quart_axisAngleH(d_Particle_Quart[i]);

         d_Particle_Pos[i]= d_Particle_Pos[i] - m_KParticleObject[ d_Particle_Type[i] ].COM;
//
//         if(i==1)
//         {
//         printf("   %d  %f %f %f  %f %f %f %f   \n",frame,d_Particle_Pos[i].x,d_Particle_Pos[i].y,d_Particle_Pos[i].z, axisangle.w, axisangle.x,axisangle.y, axisangle.z);
//         }
         glPushMatrix();
           glScalef(0.25f,0.25f,0.25f);
           glTranslatef( d_Particle_Pos[i].x,d_Particle_Pos[i].y, d_Particle_Pos[i].z);

           /* Drawing done with COM at (0,0,0) */
           glTranslatef( m_KParticleObject[ d_Particle_Type[i] ].COM.x,
        		         m_KParticleObject[ d_Particle_Type[i] ].COM.y,
        		         m_KParticleObject[ d_Particle_Type[i] ].COM.z);

             glRotatef(axisangle.w,axisangle.x,axisangle.y,axisangle.z);

           glTranslatef( -m_KParticleObject[ d_Particle_Type[i] ].COM.x,
        		         -m_KParticleObject[ d_Particle_Type[i] ].COM.y,
        		         -m_KParticleObject[ d_Particle_Type[i] ].COM.z);
           /* Draw Object */
           glCallList(10+d_Particle_Type[i]);
         //glCallList(20+d_P_ID[i]); /* color levels */
         glPopMatrix();

    } /* End loop over all particles */
 	  glFlush();
      glutSwapBuffers();

frame =0;
}/* frame check */

 /* Timing for benchmarking */
//    if( t>20.0f )
//    {
//    	exit(0);
//    }



/*--------------------------------------------------------------------------------*/
                          /* Device Interaction */
/*--------------------------------------------------------------------------------*/


    if ( m_resetsim )/* Resets the simulation */
     {
   	   m_KDevice    = new KDevice( m_KWorldObject,   m_KParticleObject,
   		 	                       m_num_KWorldObjects,
   			                       m_num_KParticleObjects, &SimInfo,&m_InitPos      );

   	   m_resetsim   = !m_resetsim;
   	   num_timeStep = 0;
     }

/*--------------------------------------------------------------------------------*/
     if( m_yImpulse )/* Makes an impulse in the y direction */
     {
       m_KDevice->IMake_Impulse();
       m_yImpulse = !m_yImpulse;
     }
/*--------------------------------------------------------------------------------*/
                 /* End of Interactive Simulation calls */
/*--------------------------------------------------------------------------------*/



/*--------------------------------------------------------------------------------*/
                /* Update Simulation on Device */
/*--------------------------------------------------------------------------------*/
     /* 2) Take a time step call to DEVICE to do dynamics */
     if(!Gl_bPause )
     {


      	  /* 2.2) DEVICE checks for collision with World and INTEGRATES */
      	    m_KDevice->IDevice_World_Interaction();

   	  /* 2.1) DEVICE calculates the forces and net velocity on all particles */
   	    m_KDevice->IDevice_Body_Body_Interaction();



   	    t +=  SimInfo.InitalDelta_t; /* Update counter */

     }

     /* Print actual time to window */
     sprintf(fps, "%s Elapsed Time ): %3.1f s",t);
     glutSetWindowTitle(fps);

     /* Take another step */
     glutPostRedisplay();

}

void Set_SimInfo()
{
	  std::cout<<" "<<std::endl;
	  std::cout<<"INFO: Setting Simulation Data "<<std::endl;

	  /* Set simulation Info*/
	   SimInfo.Num_Particles     = m_KSimulationData->get_num_Particles();
	   SimInfo.Num_ParticleTypes = m_KSimulationData->get_num_ParticleObjects();
       SimInfo.Num_WorldObjects  = m_KSimulationData->get_num_WorldObjects();
	   SimInfo.InitalDelta_t     = m_KSimulationData->get_delta_t();

	   SimInfo.globalDamping     = 0.99900f;//m_KSimulationData->get_collDamp();
	   SimInfo.gravity           = m_KSimulationData->get_gravity();
	   SimInfo.boundaryDamping   = m_KSimulationData->get_collDamp();
	   SimInfo.boundaryShear     = m_KSimulationData->get_springA();

	  /* Collision Parameters */
	   SimInfo.p_spring          = m_KSimulationData->get_springK();
	   SimInfo.p_damping         = m_KSimulationData->get_springD();
	   SimInfo.p_shear           = m_KSimulationData->get_springS();
	   SimInfo.p_attraction      = m_KSimulationData->get_springA();


	   SimInfo.Num_ParticlesType = m_KSimulationData->get_num_ParticlesType();


                /* NN GRID */
       SimInfo.cellSize          = m_KSimulationData->get_NNGrid().size;
       SimInfo.worldOrigin       = m_KSimulationData->get_NNGrid().origin;
       SimInfo.gridSize          = m_KSimulationData->get_NNGrid().num;

              /* INITAL POS */
       m_InitPos.num             = m_KSimulationData->get_InitPosGrid().num;
       m_InitPos.space           = m_KSimulationData->get_InitPosGrid().space;
       m_InitPos.start           = m_KSimulationData->get_InitPosGrid().start;


       /* Now store particle types on HOST */

	   int index_particle = 0;

	   for(int i=0; i<SimInfo.Num_ParticleTypes; i++)
	   {
		   for(int j=0; j<SimInfo.Num_ParticlesType[i]; j++)
		   {
			   m_Particle_Type[index_particle] = i;
			   index_particle++;
		   }

	   }

	   std::cout<<" Set simulation "<<std::endl;

                 /* Alternating colors */
  if( m_ColorLevels )
  {
	     int count=0;
	     float colinc=0;

	     /* color */
	     for(int i=0; i<SimInfo.Num_Particles; i++)
	     {
		    m_colorP[i] = make_float3(0.0,colinc,0.0);
            Draw_ParticleGeometryC(m_Particle_Type[i],i);

            /* Color in levels of 5 height */
		    if( count == 864 )
		    {
			   colinc = colinc +1.0f;
			   count  = 0;

			   if( colinc > 1.0f )
			   {
				 colinc=0.0f;
			   }
		     }
		     count++;
	    }
	   }/* End color Levels */


}


int main(int argc, char **argv)
{

    /* Set Up OpenGL window */
	 glutInit(&argc, argv);
	 glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
	 glutInitWindowSize(width, height);
     glutCreateWindow(" BLAZE-DEM V1.0-(Polyhedra-Sphere) ");
     glEnable(GL_DEPTH_TEST);
	 glClearColor(1.0, 1.0, 1.0, 1.0);
	 glutReportErrors();
	 glutDisplayFunc(display);
	 glutReshapeFunc(reshape);
	 glutMouseFunc(mouse);
	 glutMotionFunc(motion);
	 glutKeyboardFunc(keyboard);
	 glewInit();

	 /* OpenGL */
	 CreateEnvironment();

	 string WorldName;
	 fstream SimNameF;
	  /* Get the world name */
	 SimNameF.open("Simulation.KSim");
	 SimNameF>>WorldName;
	 SimNameF.close();


     /* Read In World File */
	 m_KSimulationData = new KSimulationData(WorldName);

	 /* Store Data from Input Class */
     m_KWorldObject         = m_KSimulationData->get_worldObject();
     m_KParticleObject      = m_KSimulationData->get_particleObject();
     m_num_KWorldObjects    = m_KSimulationData->get_num_WorldObjects();
     m_num_KParticleObjects = m_KSimulationData->get_num_ParticleObjects();
     Set_SimInfo();


     /* Create a new instance to the Device*/
     m_KDevice = new KDevice( m_KWorldObject, m_KParticleObject,
    		                  m_num_KWorldObjects, m_num_KParticleObjects,&SimInfo,&m_InitPos);

     /* Create Display Lists */
     Draw_WorldGeometry();
    Draw_ParticleGeometry();

   // PDAFIN.open("DYNA.txt");

     printf("Data Set on Device: Starting simulation \n");



  //   renderer = new ParticleRenderer;
  //   renderer->setParticleRadius(0.125f);


  glutMainLoop();

  return 0;

}

int mainF(int argc, char **argv)
{


	 string WorldName;
	 fstream SimNameF;
	  /* Get the world name */
	 SimNameF.open("Simulation.KSim");
	 SimNameF>>WorldName;
	 SimNameF.close();


     /* Read In World File */
	 m_KSimulationData = new KSimulationData(WorldName);

	 /* Store Data from Input Class */
     m_KWorldObject         = m_KSimulationData->get_worldObject();
     m_KParticleObject      = m_KSimulationData->get_particleObject();
     m_num_KWorldObjects    = m_KSimulationData->get_num_WorldObjects();
     m_num_KParticleObjects = m_KSimulationData->get_num_ParticleObjects();
     Set_SimInfo();


     /* Create a new instance to the Device*/
     m_KDevice = new KDevice( m_KWorldObject, m_KParticleObject,
    		                  m_num_KWorldObjects, m_num_KParticleObjects,&SimInfo,&m_InitPos);

     /* Create Display Lists */
     Draw_WorldGeometry();
     Draw_ParticleGeometry();


     printf("Data Set on Device: Starting simulation \n");


     printf(" CUDA ONLY \n");

     display1();


  return 0;

}
